//
// Created by az on 2020/6/25.
//

#ifndef WMR_BEZIER_PLANNER_CORRIDOR_GENERATOR_H
#define WMR_BEZIER_PLANNER_CORRIDOR_GENERATOR_H

#include <memory>

#include "data_type.h"
#include "utils.h"
#include "sdf_tools/collision_map.hpp"

using namespace std;

class CORRIDOR_GENERATOR{
protected:
    std::shared_ptr<sdf_tools::CollisionMapGrid> collision_map_;
    vector<double> map_size_;
    int max_inflate_iter_, step_length_;
    int max_x_id_, max_y_id_, max_z_id_;
    double cube_margin_, resolution_;
    bool   is_init_;
public:
    CORRIDOR_GENERATOR() {
        is_init_ = false;

        collision_map_    = std::make_shared<sdf_tools::CollisionMapGrid>();
        resolution_       = 0;
        max_inflate_iter_ = 0;
        step_length_      = 0;
        cube_margin_      = 0;

        max_x_id_ = 0;
        max_y_id_ = 0;
        max_z_id_ = 0;
    }
    CORRIDOR_GENERATOR(std::shared_ptr<sdf_tools::CollisionMapGrid>& _map, vector<double> map_size,
            const int& max_inflate_iter, const int& step_length, const double& cube_margin){
        is_init_ = true;
        map_size_.swap(map_size);

        collision_map_    = _map;
        resolution_       = collision_map_->GetResolution();
        max_inflate_iter_ = max_inflate_iter;
        step_length_      = step_length;
        cube_margin_      = cube_margin;

        max_x_id_ = (int)(collision_map_->GetXSize()/resolution_);
        max_y_id_ = (int)(collision_map_->GetYSize()/resolution_);
        max_z_id_ = (int)(collision_map_->GetZSize()/resolution_);
    }

    vector<Cube> corridorGeneration(vector<Eigen::Vector3d> path_coord, vector<double> time);
    vector<Cube> corridorGeneration(vector<Eigen::Vector3d> path_coord);
    void refreshMap(std::shared_ptr<sdf_tools::CollisionMapGrid>& _map, vector<double> map_size){
        collision_map_ = _map;
        map_size_.swap(map_size);
    }

protected:
    pair<Cube, bool> inflateCube(Cube cube, Cube lstcube);
    Cube generateCube( Eigen::Vector3d pt);
    bool isContains(Cube cube1, Cube cube2);
    void corridorSimplify(vector<Cube> & cubicList);

};

#endif //WMR_BEZIER_PLANNER_CORRIDOR_GENERATOR_H
